/*
Copyright (c) [2019年5月1日] [吴超]
[MBT_studio] is licensed under Mulan PSL v2.
You can use this software according to the terms and conditions of the Mulan PSL v2.
You may obtain a copy of Mulan PSL v2 at:
		 http://license.coscl.org.cn/MulanPSL2
THIS SOFTWARE IS PROVIDED ON AN "AS IS" BASIS, WITHOUT WARRANTIES OF ANY KIND,
EITHER EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO NON-INFRINGEMENT,
MERCHANTABILITY OR FIT FOR A PARTICULAR PURPOSE.
See the Mulan PSL v2 for more details.
*/
#include "stdafx.h"
#include "S_Px车解算.h"

using namespace physx;
using namespace std;


static physx::PxVehicleDriveTank*							g_Tank = NULL;
static physx::PxVehicleDrivableSurfaceToTireFrictionPairs*	g_FrictionPairs = NULL;
//static physx::PxBatchQuery*									g_批量查询 = NULL;
//static S_车辆场景数据查询*									g_车辆场景数据查询 = NULL;
#include <ppl.h>



static bool						gIsVehicleInAir = true;

static physx::PxF32 gSteerVsForwardSpeedData[2 * 8] =
{
	0.0f,		0.9f,
	2.0f,		0.8f,
	30.0f,		0.5f,
	120.0f,		0.1f,
	240.0f,		0.1f,
	320.0f,		0.1f,
	420.0f,		0.1f,
	420.0f,		0.1f
};
static physx::PxFixedSizeLookupTable<8> g转向前进速度表(gSteerVsForwardSpeedData, 4);

static physx::PxVehicleKeySmoothingData gKeySmoothingData = {
	{
		6.0f,	//rise rate eANALOG_INPUT_ACCEL
		6.0f,	//rise rate eANALOG_INPUT_BRAKE		
		6.0f,	//rise rate eANALOG_INPUT_HANDBRAKE	
		2.5f,	//rise rate eANALOG_INPUT_STEER_LEFT
		2.5f,	//rise rate eANALOG_INPUT_STEER_RIGHT
	},
	{
		1.0f,	//fall rate eANALOG_INPUT_ACCEL
		1.0f,	//fall rate eANALOG_INPUT_BRAKE		
		1.0f,	//fall rate eANALOG_INPUT_HANDBRAKE	
		5.0f,	//fall rate eANALOG_INPUT_STEER_LEFT
		5.0f	//fall rate eANALOG_INPUT_STEER_RIGHT
	}
};

physx::PxVehiclePadSmoothingData gPadSmoothingData = {
	{
		50.0f,		//rise rate eANALOG_INPUT_ACCEL
		50.0f,		//rise rate eANALOG_INPUT_BRAKE		
		50.0f,		//rise rate eANALOG_INPUT_HANDBRAKE	
		50.5f,		//rise rate eANALOG_INPUT_STEER_LEFT
		50.5f,		//rise rate eANALOG_INPUT_STEER_RIGHT
	},
	{
		50.0f,		//fall rate eANALOG_INPUT_ACCEL
		50.0f,		//fall rate eANALOG_INPUT_BRAKE		
		50.0f,		//fall rate eANALOG_INPUT_HANDBRAKE	
		50.0f,		//fall rate eANALOG_INPUT_STEER_LEFT
		50.0f		//fall rate eANALOG_INPUT_STEER_RIGHT
	}
};




















S_车辆场景数据查询::S_车辆场景数据查询()
	: mNumQueriesPerBatch(0),
	mNumHitResultsPerQuery(0),
	mRaycastResults(NULL),
	mRaycastHitBuffer(NULL),
	mPreFilterShader(NULL),
	mPostFilterShader(NULL)
{
	
}

S_车辆场景数据查询::~S_车辆场景数据查询()
{
}

S_车辆场景数据查询* S_车辆场景数据查询::allocate(
	const physx::PxU32 最大车辆数, const physx::PxU32 每辆车最大车轮数, 
	const physx::PxU32 maxNumHitPointsPerWheel, const physx::PxU32 numVehiclesInBatch, 
	physx::PxBatchQueryPreFilterShader preFilterShader, 
	physx::PxBatchQueryPostFilterShader postFilterShader, 
	physx::PxAllocatorCallback & allocator) 
{
	
	const PxU32 sqDataSize = ((sizeof(S_车辆场景数据查询) + 15) & ~15);

	const PxU32 maxNumWheels = 最大车辆数 * 每辆车最大车轮数;
	const PxU32 raycastResultSize = ((sizeof(PxRaycastQueryResult)*maxNumWheels + 15) & ~15);
	const PxU32 sweepResultSize = ((sizeof(PxSweepQueryResult)*maxNumWheels + 15) & ~15);

	const PxU32 maxNumHitPoints = maxNumWheels * maxNumHitPointsPerWheel;
	const PxU32 raycastHitSize = ((sizeof(PxRaycastHit)*maxNumHitPoints + 15) & ~15);
	const PxU32 sweepHitSize = ((sizeof(PxSweepHit)*maxNumHitPoints + 15) & ~15);

	const PxU32 size = sqDataSize + raycastResultSize + raycastHitSize + sweepResultSize + sweepHitSize;
	PxU8* buffer = static_cast<PxU8*>(allocator.allocate(size, NULL, NULL, 0));

	S_车辆场景数据查询* sqData = new(buffer)S_车辆场景数据查询();
	sqData->mNumQueriesPerBatch = numVehiclesInBatch * 每辆车最大车轮数;
	sqData->mNumHitResultsPerQuery = maxNumHitPointsPerWheel;
	buffer += sqDataSize;

	sqData->mRaycastResults = reinterpret_cast<PxRaycastQueryResult*>(buffer);
	buffer += raycastResultSize;

	sqData->mRaycastHitBuffer = reinterpret_cast<PxRaycastHit*>(buffer);
	buffer += raycastHitSize;

	sqData->mSweepResults = reinterpret_cast<PxSweepQueryResult*>(buffer);
	buffer += sweepResultSize;

	sqData->mSweepHitBuffer = reinterpret_cast<PxSweepHit*>(buffer);
	buffer += sweepHitSize;

	for (PxU32 i = 0; i < maxNumWheels; i++) {
		new(sqData->mRaycastResults + i) PxRaycastQueryResult();
		new(sqData->mSweepResults + i) PxSweepQueryResult();
	}

	for (PxU32 i = 0; i < maxNumHitPoints; i++) {
		new(sqData->mRaycastHitBuffer + i) PxRaycastHit();
		new(sqData->mSweepHitBuffer + i) PxSweepHit();
	}

	sqData->mPreFilterShader = preFilterShader;
	sqData->mPostFilterShader = postFilterShader;

	return sqData;
}

void S_车辆场景数据查询::free(physx::PxAllocatorCallback & allocator)
{
	allocator.deallocate(this);
}

physx::PxBatchQuery* S_车辆场景数据查询::setUpBatchedSceneQuery(const physx::PxU32 batchId, const S_车辆场景数据查询& 车辆场景查询数据, physx::PxScene* scene)
{
	const PxU32 maxNumQueriesInBatch = 车辆场景查询数据.mNumQueriesPerBatch;
	const PxU32 maxNumHitResultsInBatch = 车辆场景查询数据.mNumQueriesPerBatch*车辆场景查询数据.mNumHitResultsPerQuery;

	PxBatchQueryDesc sqDesc(maxNumQueriesInBatch, maxNumQueriesInBatch, 0);

	sqDesc.queryMemory.userRaycastResultBuffer = 车辆场景查询数据.mRaycastResults + batchId * maxNumQueriesInBatch;
	sqDesc.queryMemory.userRaycastTouchBuffer = 车辆场景查询数据.mRaycastHitBuffer + batchId * maxNumHitResultsInBatch;
	sqDesc.queryMemory.raycastTouchBufferSize = maxNumHitResultsInBatch;

	sqDesc.queryMemory.userSweepResultBuffer = 车辆场景查询数据.mSweepResults + batchId * maxNumQueriesInBatch;
	sqDesc.queryMemory.userSweepTouchBuffer = 车辆场景查询数据.mSweepHitBuffer + batchId * maxNumHitResultsInBatch;
	sqDesc.queryMemory.sweepTouchBufferSize = maxNumHitResultsInBatch;

	sqDesc.preFilterShader = 车辆场景查询数据.mPreFilterShader;

	sqDesc.postFilterShader = 车辆场景查询数据.mPostFilterShader;

	return scene->createBatchQuery(sqDesc);
}

physx::PxRaycastQueryResult * S_车辆场景数据查询::getRaycastQueryResultBuffer(const physx::PxU32 batchId) {
	return (mRaycastResults + batchId * mNumQueriesPerBatch);
}

physx::PxSweepQueryResult * S_车辆场景数据查询::getSweepQueryResultBuffer(const physx::PxU32 batchId) {
	return (mSweepResults + batchId * mNumQueriesPerBatch);
}

physx::PxU32 S_车辆场景数据查询::getQueryResultBufferSize() const {
	return mNumQueriesPerBatch;
}










/*physx::PxQueryHitType::Enum WheelSceneQueryPreFilterBlocking(
	physx::PxFilterData	filterData0,
	physx::PxFilterData filterData1,
	const void*			constantBlock,
	physx::PxU32		constantBlockSize,
	physx::PxHitFlags&	queryFlags
) {
	PX_UNUSED(filterData0);
	PX_UNUSED(constantBlock);
	PX_UNUSED(constantBlockSize);
	PX_UNUSED(queryFlags);
	return ((0 == (filterData1.word3 & DRIVABLE_SURFACE)) ? physx::PxQueryHitType::eNONE : physx::PxQueryHitType::eBLOCK);
}*/





void S_Px车模拟::f_初始化车辆解算(S_Px物理引擎核心& Px核心, physx::PxDefaultAllocator& allocator, physx::PxScene* scene) {
	PxMaterial* mat = Px核心.m_Physics->createMaterial(0.6, 0.6, 0.8);
	g_FrictionPairs = f_创建轮胎摩擦面(mat);


	m_车辆查询 = S_车辆场景数据查询::allocate(100, PX_MAX_NB_WHEELS, 100, 20, WheelSceneQueryPreFilterBlocking, NULL, allocator);
	m_批量查询 = S_车辆场景数据查询::setUpBatchedSceneQuery(10, *m_车辆查询, scene);

}

void S_Px车模拟::f_模拟(physx::PxFoundation * Px核心, physx::PxScene* scene, S_Px车** 车, uint32 车数量, float32 步进时间)
{
	if (车[0]->m_是否手柄操作) {
		PxVehicleDrive4WSmoothAnalogRawInputsAndSetAnalogInputs(gPadSmoothingData, g转向前进速度表, 车[0]->m_VehicleInputData, 步进时间, gIsVehicleInAir, *车[0]->m_车辆驱动);
	}
	else {
		PxVehicleDrive4WSmoothDigitalRawInputsAndSetAnalogInputs(gKeySmoothingData, g转向前进速度表, 车[0]->m_VehicleInputData, 步进时间, gIsVehicleInAir, *车[0]->m_车辆驱动);
	}


//#pragma loop(vector_on)
	vector<physx::PxVehicleWheels*>				vehicles(车数量);
	vector<physx::PxVehicleWheelQueryResult>	车轮(车数量);


	physx::PxWheelQueryResult wheelQueryResults[PX_MAX_NB_WHEELS];
	//Concurrency::parallel_for<uint32>(0, 车数量, [&](uint32 i) {
	for (uint32 i = 0; i < 车数量; ++i) {
		vehicles[i]	= 车[i]->m_车辆驱动;
		车轮[i]		= { &wheelQueryResults[i], 车[i]->m_车辆驱动->mWheelsSimData.getNbWheels() };
	}
	//});
	

	//auto mWheelQueryResults = SampleVehicleWheelQueryResults::allocate(MAX_NUM_4W_VEHICLES * 4 + MAX_NUM_6W_VEHICLES * 6);

	//PxVehicleSuspensionSweeps
	physx::PxRaycastQueryResult*	raycastResults		= m_车辆查询->getRaycastQueryResultBuffer(0);
	const physx::PxU32				raycastResultsSize	= m_车辆查询->getQueryResultBufferSize();
	PxVehicleSuspensionRaycasts(m_批量查询, 车数量, vehicles.data(), raycastResultsSize, raycastResults);

	const physx::PxVec3 grav = scene->getGravity();
	//physx::PxWheelQueryResult wheelQueryResults[PX_MAX_NB_WHEELS];
	//physx::PxVehicleWheelQueryResult vehicleQueryResults[1] = { { wheelQueryResults, 车[0]->m_车辆驱动->mWheelsSimData.getNbWheels() } };
	PxVehicleUpdates(步进时间, grav, *g_FrictionPairs, 车数量, vehicles.data(), 车轮.data());


//车辆是否在空中
//#pragma loop(vector_on)
	Concurrency::parallel_for<uint32>(0, 车数量, [&](uint32 i) {
	//for (uint32 i = 0; i < 车数量; ++i) {
		gIsVehicleInAir = 车[i]->m_车辆驱动->getRigidDynamicActor()->isSleeping() ? false : PxVehicleIsInAir(车轮[0]);
	//}
	});

}


